#include <GL/glut.h>
#include <cstdlib>
#include <iostream>
#include <sstream>

#include "arcball.h"
#include "atmatrix.h"
#include "Skeleton.h"
#include "Vector.h"

using namespace std;

Skeleton *skel;
Skeleton::Pose pose;
int current_frame = 0, motion_size;

bool do_play, show_axis;

int winw = 700, winh = 700; // window size
int xini, yini; // initial mouse position
double xwini, ywini, zwini; // initial mouse position in world coordinates
int buttonpressed; // 

AMatrix<GLfloat> sceneT;
AMatrix<GLfloat> sceneIniT;
ArcBall arcball;

/// time info
float cfps;
time_t start, end;
unsigned frame;

/// Info variables
void* fontsmall= GLUT_BITMAP_HELVETICA_12;
string frames("Frame: ");
string fps("FPS: ");
string options("Zoom: + / -");

/// Renders info about the model
void renderInfo();
/// Returns the world coordinates of a point in screen space
void screenToWorld(int x, int y, double &xw, double &yw, double &zw);
/// Implements the "zoomin" operation. Narrows the view of the scene.
void zoomIn();
/// Implements the "zoomout" operation. Enlarges the view of the scene.
void zoomOut();

/// OpenGL display function
void display(void) {

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glLoadIdentity();

	glMultMatrixf(&sceneT);

	glDisable(GL_LIGHTING);
	glColor3d(0.0, 0.5, 0.0);
	glBegin(GL_QUADS);
	glVertex3d(-100.0, skel->minPoint.y, -100.0);
	glVertex3d(100.0, skel->minPoint.y, -100.0);
	glVertex3d(100.0, skel->minPoint.y, 100.0);
	glVertex3d(-100.0, skel->minPoint.y, 100.0);
	glEnd();

	//    glColor3f(0.0, 0.0, 0.0);
	//    glPointSize(10.0);
	//    glBegin(GL_POINTS);
	//    int x = current_frame + 1;
	//    if(x >= motion_size) x--;
	//    for(Skeleton::Joints::iterator it = skel->joints[x].begin();
	//                    it != skel->joints[x].end(); it++){
	//        Vector3d vec = it->second;
	//        glVertex3d(vec.x, vec.y, vec.z);
	//    }
	//    glEnd();

	glLineWidth(5.0);
	glBegin(GL_LINES);
	for (Skeleton::Pose::iterator it = pose.begin(); it != pose.end(); it++) {
		Vector3d color = skel->getBoneColor(it->first);
		if (show_axis)
			glColor3d(0.0, 0.0, 0.0);
		else
			glColor3d(color.x, color.y, color.z);
		glVertex3d(it->second.first.x, it->second.first.y, it->second.first.z);
		glVertex3d(it->second.second.x, it->second.second.y, it->second.second.z);
		if (show_axis) {
			Bone bone = skel->getSkeleton(current_frame)[it->first];
//			cout << bone.getName() << ": " << bone.getAxis_X() << bone.getAxis_Y() << bone.getAxis_Z() << endl;
			glColor3d(1.0, 0.0, 0.0);
			glVertex3d(it->second.first.x, it->second.first.y, it->second.first.z);
			glVertex3d(it->second.first.x + bone.getAxis_X().x, it->second.first.y + bone.getAxis_X().y, it->second.first.z + bone.getAxis_X().z);
			glColor3d(0.0, 1.0, 0.0);
			glVertex3d(it->second.first.x, it->second.first.y, it->second.first.z);
			glVertex3d(it->second.first.x + bone.getAxis_Y().x, it->second.first.y + bone.getAxis_Y().y, it->second.first.z + bone.getAxis_Y().z);
			glColor3d(0.0, 0.0, 1.0);
			glVertex3d(it->second.first.x, it->second.first.y, it->second.first.z);
			glVertex3d(it->second.first.x + bone.getAxis_Z().x, it->second.first.y + bone.getAxis_Z().y, it->second.first.z + bone.getAxis_Z().z);
		}
	}
	glEnd();
	
//	exit(0);

//	glLineWidth(1.0);
//	glBegin(GL_LINES);
//	glColor3d(0.0, 0.0, 0.0);
//	Vector3d p;
//	for (int i = 1; i < (int) skel->joints.size() - 1; i++) {
//		p = skel->joints[i]["lhand"];
//		glVertex3d(p.x, p.y, p.z);
//		p = skel->joints[i + 1]["lhand"];
//		glVertex3d(p.x, p.y, p.z);
//	}
//	glEnd();

	glEnable(GL_LIGHTING);

	renderInfo();
	glutSwapBuffers();
}

/// Mouse press function
void mouseclick(int button, int state, int x, int y) {

	xini=x;
	yini=winh-y;
	if (state == GLUT_DOWN) {
		buttonpressed = button;
		if (buttonpressed == GLUT_MIDDLE_BUTTON) {
			arcball.click(xini, yini);
		} else if (buttonpressed == GLUT_RIGHT_BUTTON) {
			screenToWorld(x, winh-y, xwini, ywini, zwini);
		}
		sceneIniT = sceneT;
	} else if (state == GLUT_UP) {
	}
	glutPostRedisplay();
}

/// Mouse move function
void mousemove(int x, int y) {

	if (buttonpressed == GLUT_MIDDLE_BUTTON) { // rotations handler
		AMatrix<float> mT;
		arcball.drag(x, winh-y, &mT);
		sceneT = mT*sceneIniT;
	} else if (buttonpressed == GLUT_RIGHT_BUTTON) { // translations handler
		double xw, yw, zw;
		screenToWorld(x, winh-y, xw, yw, zw);
		AMatrix<float> mT;
		mT.identity();
		mT.translation(xw-xwini, yw-ywini, zw-zwini);
		sceneT = mT*sceneIniT;
	}
	glutPostRedisplay();
}

/// OpenGL key press function
void keypress(unsigned char key, int x, int y) {

	switch (key) {
		case 27:
			exit(1);
			break;
		case 'p': {
			do_play = not do_play;
			break;
		}
		case 'x': {
			show_axis = not show_axis;
			break;
		}
		case '.':
			current_frame++;
			//~ cout << "++" << current_frame << endl;
			if (current_frame >= motion_size)
				current_frame = 0;
			pose = skel->getPose(current_frame);
			break;
		case ',':
			current_frame--;
			//~ cout << "--" << current_frame << endl;
			if (current_frame < 0)
				current_frame = motion_size - 1;
			pose = skel->getPose(current_frame);
			break;
		case '+': {
			zoomIn();
			break;
		}
		case '-': {
			zoomOut();
			break;
		}
	}
	glutPostRedisplay();
}

/// Returns the world coordinates of a point in screen space
void screenToWorld(int x, int y, double &xw, double &yw, double &zw) {

	GLdouble mMtx[16], pMtx[16]; /// modelview/projection matrix
	GLint viewport[4]; /// the viewport
	glGetIntegerv(GL_VIEWPORT, viewport);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glGetDoublev(GL_MODELVIEW_MATRIX, mMtx);
	glGetDoublev(GL_PROJECTION_MATRIX, pMtx);

	gluUnProject(x, y, 1.0, mMtx, pMtx, viewport, &xw, &yw, &zw);
}

/// OpenGL reshape funcion
void reshape(int w, int h) {

	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	glOrtho(-1, 1, -1, 1, -4, 4);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	winw = w;
	winh = h;
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	glutPostRedisplay();

	arcball.setBounds(winw, winh);
}

/// OpenGL initializations
void init(void) {

	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);
	glEnable(GL_LIGHT1);
	glEnable(GL_DEPTH_TEST);
	glEnable(GL_NORMALIZE);

	// lighting setup
	GLfloat left_light_position[] = { 0.0f, 2.0f, 2.0f, 0.0f };
	GLfloat right_light_position[] = { 0.0f, -2.0f, 2.0f, 0.0f };
	GLfloat left_diffuse_light[] = { 1.0f, 1.0f, 1.0f, 1.0f };
	GLfloat right_diffuse_light[] = { 1.0f, 1.0f, 1.0f, 1.0f };
	GLfloat color_front[] = { 0.0f, 0.5f, 1.0f, 1.0f };
	GLfloat colr_back[] = { 1.0f, 0.0f, 0.0f, 1.0f };

	glLightfv(GL_LIGHT0, GL_POSITION, left_light_position);
	glLightfv(GL_LIGHT1, GL_POSITION, right_light_position);
	glLightfv(GL_LIGHT0, GL_DIFFUSE, left_diffuse_light);
	glLightfv(GL_LIGHT1, GL_DIFFUSE, right_diffuse_light);
	glMaterialfv(GL_FRONT, GL_DIFFUSE, color_front);
	glMaterialfv(GL_BACK, GL_DIFFUSE, colr_back);

	glClearColor(1, 1, 1, 1);
	glColor4f(1, 1, 0, 1);
	sceneT.identity();

	AMatrix<float> mT;
	mT.identity();
	mT.scaling(1.0/24);
	sceneT = mT*sceneT;

	std::time(&start);
	frame = 0;

	//~ cout << "getPose" << endl;
	pose = skel->getPose(0);
	//~ cout << "getMotionSize" << endl;
	motion_size = skel->getMotionSize();
	//~ cout << "motion size: " << motion_size << endl;

	show_axis = do_play = false;
}

/// Implements the "zoomin" operation. Narrows the view of the scene.
void zoomIn() {

	AMatrix<float> mT;
	mT.identity();
	mT.scaling(1.2);
	sceneT = mT*sceneT;
}

/// Implements the "zoomout" operation. Enlarges the view of the scene.
void zoomOut() {

	AMatrix<float> mT;
	mT.identity();
	mT.scaling(1.0/1.2);
	sceneT = mT*sceneT;
}

/// Renders a string
void renderBitmapString(float x, float y, float z, void *font, const char *string) {
	const char *c;
	glRasterPos3f(x, y, z);
	for (c=string; *c != '\0'; c++) {
		glutBitmapCharacter(font, *c);
	}
}

/// Renders info about the animation
void renderInfo() {

	glLoadIdentity();
	glDisable(GL_LIGHTING);
	glDisable(GL_DEPTH_TEST);
	glColor4f(0, 0, 0, 1);
	renderBitmapString(-0.9, 0.85, 0, (void *)fontsmall, frames.c_str());
	renderBitmapString(-0.9, 0.80, 0, (void *)fontsmall, fps.c_str());
	renderBitmapString(-0.9, 0.75, 0, (void *)fontsmall, options.c_str());
	glEnable(GL_LIGHTING);
	glEnable(GL_DEPTH_TEST);
}

/// timer func
void timer(int time) {
	//~ void idle(){
	if (do_play) {
		current_frame++;
		if (current_frame >= motion_size)
			current_frame = 0;
		pose = skel->getPose(current_frame);
	}

	ostringstream fm1, fm2;
	fm1 << current_frame + 1;
	fm2 << motion_size;
	frames = "Frames: " + fm1.str() + " / " + fm2.str();

	/// FPS info
	frame++;
	std::time(&end);
	double ddifftime = std::difftime(end, start);
	if (ddifftime > 1) {
		cfps = frame/ddifftime;
		start = end;
		frame = 0;
		ostringstream s1;
		s1<<cfps;
		fps = "FPS: " + s1.str();
	}

	glutPostRedisplay();
	glutTimerFunc(time, timer, time);
}

void showUsage(string progName) {
	cout << "Usage: " << progName << " (file.bvh | skeleton.asf motion.amc) [-o arq.pss skelInFile.skel]\n";
}

int main(int argc, char **argv) {

	if (argc < 2) {
		showUsage(argv[0]);
		return (0);
	}

	string asfFile, amcFile, bvhFile, skelInFile, pssFile;
	int option = 0;
	bool toExport = false;

	for (int i = 1; i < argc; i++) {
		string param = argv[i];
		if (param == "-o") {
			if (i + 1 < argc) {
				i = i + 1;
				param = argv[i];
				if (param.substr(param.size() - 4, 4) == ".pss") {
					pssFile = param;
				}
			} else {
				showUsage(argv[0]);
				return 0;
			}
			if (i + 1 < argc) {
				i = i + 1;
				param = argv[i];
				if (param.substr(param.size() - 5, 5) == ".skel") {
					skelInFile = param;
				}
			} else {
				showUsage(argv[0]);
				return 0;
			}
			toExport = true;
		} else if (param.substr(param.size() - 4, 4) == ".asf") {
			asfFile = param;
			if (i + 1 < argc) {
				i = i + 1;
				param = argv[i];
				if (param.substr(param.size() - 4, 4) == ".amc") {
					amcFile = param;
				} else {
					showUsage(argv[0]);
					return 0;
				}
			} else {
				showUsage(argv[0]);
				return 0;
			}
			option += 1;
		} else if (param.substr(param.size() - 4, 4) == ".bvh") {
			bvhFile = param;
			option += 2;
		} else {
			showUsage(argv[0]);
			return 0;
		}
	}

	if (toExport && !option) {
		showUsage(argv[0]);
		return 0;
	}

	switch (option) {
		case 1:
			skel = new Skeleton(asfFile, amcFile);
			break;
		case 2:
			skel = new Skeleton(bvhFile);
			break;
		case 3:
			showUsage(argv[0]);
			return 0;
		default:
			break;
	}

	if (toExport) {
		skel->exportPSS(skelInFile, pssFile);
	}

	// Glut setup
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH);
	glutInitWindowSize(winw, winh);
	glutInitWindowPosition(150, 0);
	glutCreateWindow("glutMOCAPviewer");

	// OpenGL and data initialization
	init();

	// Callbacks registration
	glutDisplayFunc(display);
	glutReshapeFunc(reshape);
	glutMouseFunc(mouseclick);
	glutMotionFunc(mousemove);
	glutKeyboardFunc(keypress);
	glutTimerFunc(6, timer, 6);
	glutMainLoop();

	return 0;
}
